#!/usr/bin/python
import sys
reload(sys)
sys.setdefaultencoding('utf8')
import rospy
from geometry_msgs.msg import Twist,Point,Quaternion
from nav_msgs.msg import Odometry
from kykt.msg import gpsMsg
import math
import time
import tf
class Pid_t:
    def __init__(self,kp,ki,kd,max,T = 0.01):
        self.kp = kp
        self.ki = ki
        self.kd = kd
        self.max = max
        self.ek = 0
        self.ek1 = 0
        self.ek2 = 0
        self.uk1 = 0
        self.res = 0
        self.T = T
    def cal(self,current,target):
        self.ek=target-current
        try:
            delta_uk=self.kp*(self.ek-self.ek1)+self.ki*self.T*self.ek+self.kd/self.T*(self.ek-2*self.ek1+self.ek2)
        except:
            delta_uk=self.kp*(self.ek-self.ek1)+self.ki*self.T*self.ek
        self.uk1 += delta_uk
        res = self.uk1
        if math.fabs(res) >= max:
            res = max*math.fabs(res)/res
        self.ek2=self.ek1
        self.ek1=self.ek
        return res

class ControlNode:
    def __init__(self):
    
        rospy.init_node('control_node', anonymous = True)
        
        self.control_rate = rospy.get_param("/control_rate")
        self.rate = rospy.Rate(self.control_rate)

        self.way_points = rospy.get_param("/way_points")
        self.way_points_index = 0

        # self.way_points = [[39.978450,116.344425],[39.978450,116.344440],[39.978450,116.344460],[39.978450,116.344490],[39.978450,116.344530],[39.978450,116.344590]]
        # self.way_points_index = 0

        self.speed_p = rospy.get_param("/speed_p")
        self.speed_i = rospy.get_param("/speed_i")
        self.speed_d = rospy.get_param("/speed_d")
        self.speed_max = rospy.get_param("/speed_max")
        self.speed_pid = Pid_t(self.speed_p,self.speed_i,self.speed_d,self.speed_max,1/self.control_rate)

        self.cmd_vel_pub = rospy.Publisher('cmd_vel',Twist,queue_size=10)
        self.cmd_vel = Twist()
        
        rospy.Subscriber('odom',Odometry,self.get_odom_cb)
        self.current_position = Odometry().pose.pose.position
        self.current_orientation = Odometry().pose.pose.orientation
        self.current_orientation_rpy = [0,0,0]

        rospy.Subscriber('gps',gpsMsg,self.get_gps_cb)
        self.gps_flag = False
        rospy.loginfo('Start control_node')

    def wgs2enu(self,last_Longitude,last_Latitude,Longitude,Latitude):
        # r = 6378137/1000000.0
        r = 6378137
        dx = (Longitude-last_Longitude)*math.pi/180*r*math.cos(Latitude*math.pi/180)
        dy = (Latitude-last_Latitude)*math.pi/180*r
        return [dx,dy]

    def wayPointsControl(self):
        # if self.gps_flag == False:
        #     return
        # target_Longitude = float(self.way_points[self.way_points_index][1])
        # target_Latitude = float(self.way_points[self.way_points_index][0])
        # [dx,dy] = self.wgs2enu(target_Longitude,target_Latitude,self.current_Longitude,self.current_Latitude)


        target_x = float(self.way_points[self.way_points_index][0])
        target_y = float(self.way_points[self.way_points_index][1])
        current_x = self.current_position.x
        current_y = self.current_position.y
        dx = target_x - current_x
        dy = target_y - current_y


        print("dx:%f,dy:%f,index:%d"%(dx,dy,self.way_points_index))
        # print("tar_lon:%f,tar_lat:%f,cur_lon:%f,cur_lat:%f"%(target_Longitude,target_Latitude,self.current_Longitude,self.current_Latitude))

        dis = math.sqrt(dx*dx+dy*dy)
        eps = 0.05
        if dis < eps:
            self.way_points_index += 1
            rospy.loginfo("Have arrived the %d point."%(self.way_points_index))
            if self.way_points_index >= len(self.way_points):
                self.way_points_index = len(self.way_points) - 1
                rospy.loginfo("Have arrived the last point.")
        else:
            alpha = math.atan2(dy,dx)-self.current_orientation_rpy[2]
            L = 0.2
            angle_vel = math.atan2(2*L*math.sin(alpha),dis)
            speed_vel = -self.speed_pid.cal(dis,0)
            self.cmd_vel.linear.x = speed_vel
            self.cmd_vel.angular.z = angle_vel
            print("vel:%f,angle:%f"%(speed_vel,angle_vel))
            self.cmd_vel_pub.publish(self.cmd_vel)
    
    def get_gps_cb(self,data):
        if self.gps_flag == False:
            self.gps_flag = True
        self.current_Latitude = float(data.Latitude)
        self.current_Longitude = float(data.Longitude)
    
    def get_odom_cb(self,data):
        self.current_position = data.pose.pose.position         # .x .y
        self.current_orientation = data.pose.pose.orientation   # .x .y .z .w
        self.current_orientation_rpy = self.euler_from_quaternion(self.current_orientation)

    def euler_from_quaternion(self,orientation):
        x = orientation.x
        y = orientation.y
        z = orientation.z
        w = orientation.w
        [r,p,y] = tf.transformations.euler_from_quaternion((x,y,z,w))
        # r = math.atan2(2*(w*x+y*z),1-2*(x*x+y*y))
        # p = math.asin(2*(w*y-z*z))
        # y = math.atan2(2*(w*z+x*y),1-2*(z*z+y*y))
        return [r,p,y]

    def MainLoop(self):
        while not rospy.is_shutdown():
            # self.cmd_vel_pub.publish(self.cmd_vel)
            self.wayPointsControl()
            self.rate.sleep()
            
if __name__ == '__main__':
    controlNode = ControlNode()
    controlNode.MainLoop()
